#include "filter.h"

#if (FILTER_KALMAN_ENABLE == 1)
void KalmanInt_Init(Kalman_t *kfp)
{
    kfp->A = 8;
    kfp->Last_P = 0;
    kfp->Now_P = 0;
    kfp->out = 0;
    kfp->Kg = 0;
    kfp->Q = 200;
    kfp->R = 10;
}

/**
 *卡尔曼滤波器
 *@param 	Kalman *kfp 卡尔曼结构体参数
 *   			int input 需要滤波的参数的测量值（即传感器的采集值）
 *@return 滤波后的参数（最优值）
 */
int KalmanIntFilter(Kalman_t *kfp, int input)
{
    // 预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
    kfp->Now_P = kfp->Last_P + kfp->Q;
    // 卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
    kfp->Kg = (kfp->Now_P << kfp->A) / (kfp->Now_P + kfp->R);
    // 更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
    kfp->out = kfp->out + ((kfp->Kg * (input - kfp->out)) >> kfp->A); // 因为这一次的预测值就是上一次的输出值
    // 更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
    kfp->Last_P = (((1 << kfp->A) - kfp->Kg) * kfp->Now_P) >> kfp->A;
    return kfp->out;
}

#endif 


/**
*  中位值平均滤波
* @param pData:没有滤波的数据 
* @param nSize:数据大小 
* @return:滤波数值
*/
int Filter_Median(int* pData,int nSize)
{
    int max,min;
    int sum;
    if(nSize>2)
    {
        max = pData[0];
        min = max;
        sum = 0;
        for(int i=0;i<nSize;i++)
        {
            sum += pData[i];            
            if(pData[i]>max)
            {
                max = pData[i];   //一个循环之后max就是最大的值
            }

            if(pData[i]<min)
            {
                min = pData[i];   //一个循环之后min就是最小的值
            }
        }

        sum = sum-max-min;       //去掉最大的值和最小的值
        return sum/(nSize-2); //对N-2个数求平均值          
    }

    return 0;
}

